from RR_api import *
import math
import time
import subprocess

global rr

##########################################################
# Initialization
##########################################################
def Initialize_Vision_API():
    global bioloidX, bioloidY,bioloidR, bioloidvalid, targetX,targetY, target,width,height,rr
    # initialize the API class

    RR_port = "-api_port 6060"
    print "opening API"
    try:
        #Changed to match my path
        process = subprocess.Popen(['C:\Program Files\RoboRealm\Roborealm.exe', RR_port])
        time.sleep(0.5)
        print "Camera started successfully"
    except:
        print "No Camera Connected"
        print "Check path in Popen statement to make sure it points to your Roborealm executable"
        
    
    # initialize the API class
    time.sleep(1.0)
    rr = RR_API()

    #connect to RoboRealm  Make sure that Roboream is open and Running the API server
    try:
        rr.Connect("localhost")
    except ValueError:
        print "API Server is not running!"
        sys.exit()
    time.sleep(1)
    bioloidX = float(0.0)
    bioloidY = float(0.0)
    bioloidR = float(0.0)
    bioloidvalid = False

    targetX = float(0.0)
    targetY = float(0.0)
    target = 0;


    rr.Execute("<head><version>2.5.2</version></head><Camera_Properties><camera>USB Video Device</camera> \
    <software_auto_exposure>FALSE</software_auto_exposure><video_rate>10</video_rate><gamma_value>1</gamma_value> \
    <video_size>640 x 480</video_size><video_format>YUY2</video_format></Camera_Properties>")
    time.sleep(1)
    width, height = rr.GetDimension()
    width = float(width)
    height = float(height)

    rr.Execute("<Fiducial>  <min_confidence>10</min_confidence>  <min_size>0</min_size>  <max_size>100</max_size> \
    <sort_array_on_y>FALSE</sort_array_on_y>  <display_rx>FALSE</display_rx>  <display_confidence>TRUE</display_confidence> \
    <max_angle>360</max_angle>  <matched_color_index>3</matched_color_index>  <display_orientation>TRUE</display_orientation> \
    <test_color_index>5</test_color_index>  <select_top_blobs>10</select_top_blobs>  <create_shape_array>TRUE</create_shape_array>\
    <min_angle>0</min_angle>  <occurrence_threshold>0</occurrence_threshold>  <display_ty>FALSE</display_ty> \
    <max_confidence>100</max_confidence>  <perspective>256</perspective>  <display_scale>FALSE</display_scale> \
    <base_folder>.\\fiducials</base_folder>  <display_coord>TRUE</display_coord>  <display_filename>FALSE</display_filename> \
    <sort_array_on_x>FALSE</sort_array_on_x>  <display_ry>FALSE</display_ry>  <display_tx>FALSE</display_tx> \
    <font_size_index>4</font_size_index></Fiducial><Mouse>  <x_position_max>640</x_position_max> \
    <y_position_max>480</y_position_max>  <delay_write>TRUE</delay_write></Mouse> \
    <Set_Variable>  <set_1_once>TRUE</set_1_once>  <set_3_once>FALSE</set_3_once>  \
    <value_1>0</value_1>  <variable_2>ROTATION</variable_2>\
    <set_4_once>FALSE</set_4_once>  <set_2_once>TRUE</set_2_once>  <value_2>0</value_2>\
    <variable_1>SPEED</variable_1></Set_Variable>\
    <Keyboard_Read>  <KEY_3>[cursor right]</KEY_3><VARIABLE_0>SPEED</VARIABLE_0><CLEAR_2>FALSE</CLEAR_2>\
    <ACTION_0>ADD</ACTION_0><VALUE_1>1</VALUE_1>  <KEY_2>[cursor left]</KEY_2>  <CLEAR_1>FALSE</CLEAR_1>\
    <VARIABLE_1>SPEED</VARIABLE_1><ACTION_1>SUBTRACT</ACTION_1><KEY_1>[cursor down]</KEY_1><CLEAR_0>FALSE</CLEAR_0>\
    <VALUE_2>1</VALUE_2><VARIABLE_2>ROTATION</VARIABLE_2>  <ACTION_2>ADD</ACTION_2>\
    <KEY_0>[cursor up]</KEY_0>  <VALUE_3>1</VALUE_3> <CLEAR_3>FALSE</CLEAR_3><VARIABLE_3>ROTATION</VARIABLE_3>\
    <VALUE_0>1</VALUE_0>  <ACTION_3>SUBTRACT</ACTION_3>   \
    <MAX_0>16</MAX_0><MAX_2>16</MAX_2><MIN_1>-16</MIN_2> <MIN_3>-16</MIN_3></Keyboard_Read>")
    time.sleep(.1)


def Get_Speed():
    global rr

    SPEED = rr.GetVariable("SPEED")
    orient = rr.GetVariable("ROTATION")
    
    print "Get ",SPEED, orient

    return SPEED, orient

# get marker
def Get_Marker(markername):
  global bioloidX, bioloidY,bioloidR, bioloidvalid, targetX,targetY, target,width,height,rr

  bioloidvalid = False
  target = False
  
  rr.WaitImage(0)
#  newclock = time.clock()
#  print 1/(newclock-prevclock), "fps"
#  prevclock = newclock

  fiducialsvar = rr.GetVariable("FIDUCIALS").split(",")
  fiducialspath = rr.GetVariable("FIDUCIALS_PATH").split("\\")
  nmarks = len(fiducialsvar)/17
  for ind in range(0,nmarks):
    if( (len(fiducialspath)-1) == nmarks  ):
      name = fiducialspath[ind+1].split(".")[0]
      if (name==markername):
        bioloidX = float(fiducialsvar[9+ind*17])+width/2
        bioloidY = float(fiducialsvar[10+ind*17])+height/2
        bioloidR = float(fiducialsvar[14+ind*17])*math.pi/180.0
        bioloidvalid = True;


  return bioloidX,bioloidY,bioloidR, bioloidvalid  

def Bioloid_Visual_Navigation_Control():
  global bioloidX, bioloidY,bioloidR, bioloidvalid, targetX,targetY, target,width,height,rr
    #############################################################
    #  Timer Loop
    #############################################################
  SPEED = 0
  orient = 0

  bioloidX, bioloidY, bioloidR, bioloidvalid = Get_Marker("bioloid")

  targetX = rr.GetVariable("MOUSE_CLICK_X")
  targetY = rr.GetVariable("MOUSE_CLICK_Y")

  if( len(targetX)>0 ):
    targetX = float(targetX)
    targetY = float(targetY)
    target = True
  else:
    target = False
  print "Target (", targetX, ",", targetY, ")",\
        "Bioloid (", bioloidX, ",", bioloidY, "," , bioloidR , ")"

  #Define controller
  if( target and bioloidvalid ):
    print "controller"

    if bioloidR > math.pi:
      bioloidR = bioloidR - 2*math.pi
      
    #orientation
    errorient = math.atan2( targetY-bioloidY, targetX-bioloidX) + bioloidR
    if (errorient < -math.pi):
      errorient = errorient + 2 * math.pi

    if (errorient > math.pi/4 ):
      orient = 15
    elif (errorient > 0 ):
      orient = 10
    elif (errorient > -math.pi/4 ):
      orient = -10
    else:
      orient = -15

    if abs(errorient)<math.pi/8:
      orient = 0

    #distance
    errordist = math.sqrt( math.pow(targetY-bioloidY,2) + math.pow(targetX-bioloidX,2));

    SPEED = errordist;
    if abs(errordist) < 30:
      SPEED = 0
      orient = 0
    elif abs(errordist) < 80 and orient != 0:
        SPEED = 3
    elif abs(orient)>10:
      SPEED = 5
    else:
      SPEED = 9


    print errordist,errorient,SPEED,orient
  return SPEED,orient


    

